#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   dual_arm_state_pub.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   1.1
@Create Time    :   CURRENT_YEAR/CURRENT_MONTH/CURRENT_DATE CURRENT_HOUR:CURRENT_MINUTE:CURRENT_SECOND
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   None 
'''

# here put the import lib

import dual_arm_manager

import rospy  
from sensor_msgs.msg import JointState
from joint_state_exchange.msg import RobotState
from dual_arm_base_control.msg import TelemetryInfo
from std_msgs.msg import String


dfr_nh = dual_arm_manager.dual_arm

# TODO wrist and gripper angle info 

def main():
    
    samplingFrequency = dual_arm_manager.SAMPLING_FREQUENCY
    if dfr_nh:
        rospy.loginfo("dual_arm_manager.dual_arm node handler import success!")
        pub_servo_info(samplingFrequency)
    else:
        rospy.loginfo("dual_arm_manager.dual_arm node handler import Failed!")

def pub_servo_info(samplingFrequency):
    # 创建舵机关节信息发布者话题
    # main function to construct new control threading
    rospy.loginfo("State info Pub Node init success!")

    # 临时变量
    angle_tmp = []
    power_tmp = []
    tempture_tmp = []
    
    for SERVO in dfr_nh.get_arm_servo_list():
        angle_tmp.append(dfr_nh.read_single_servo_angle(SERVO_ID=SERVO))
        power_tmp.append(dfr_nh.get_servo_status(SERVO_ID=SERVO).get('power'))
	tempture_tmp.append(dfr_nh.get_servo_status(SERVO_ID=SERVO).get('temp'))
    rospy.loginfo(angle_tmp)

    rospy.init_node('dual_arm_state_pub_node', anonymous=True)
    rospy.loginfo("Node init success!")
    servo_pub = rospy.Publisher('/servo_info',
                                TelemetryInfo,
                                queue_size = 5)
    pub_rate = rospy.Rate(1)         # pub frequence 5 
    #### Create TelemetryInfo ####
    msg =TelemetryInfo()
    rospy.loginfo('Robot Telemetry Info Publishing')
    while not rospy.is_shutdown():
        # publish the servo angle and other info
        #rospy.loginfo('Robot Telemetry Info Publishing')

        for SERVO in dfr_nh.get_arm_servo_list():
            angle_tmp.append(dfr_nh.read_single_servo_angle(SERVO_ID=SERVO))
            power_tmp.append(dfr_nh.get_servo_status(SERVO_ID=SERVO).get('power'))
            tempture_tmp.append(dfr_nh.get_servo_status(SERVO_ID=SERVO).get('temp'))
        #rospy.loginfo(angle_tmp,power_tmp, tempture_tmp)
	#print(angle_tmp,power_tmp, tempture_tmp)
        
        #### Create TelemetryInfo msg ####
        msg.header.stamp = rospy.Time.now()
        msg.leftArmJointAngle.append(angle_tmp[0])
        msg.leftArmJointAngle.append(angle_tmp[1])
        msg.leftArmJointAngle.append(angle_tmp[2])
        msg.leftArmJointAngle.append(0)
        msg.leftArmJointAngle.append(0)

        msg.rightArmJointAngle.append(angle_tmp[3])
        msg.rightArmJointAngle.append(angle_tmp[4])
        msg.rightArmJointAngle.append(angle_tmp[5])
        msg.rightArmJointAngle.append(0)
        msg.rightArmJointAngle.append(0)
        
        msg.leftArmJointPower.append(power_tmp[0])
        msg.leftArmJointPower.append(power_tmp[1])
        msg.leftArmJointPower.append(power_tmp[2])
        msg.leftArmJointPower.append(0)
        msg.leftArmJointPower.append(0)

        msg.rightArmJointPower.append(power_tmp[3])
        msg.rightArmJointPower.append(power_tmp[4])
        msg.rightArmJointPower.append(power_tmp[5])
        msg.rightArmJointPower.append(0)
        msg.rightArmJointPower.append(0)

        msg.leftArmJointTemp.append( tempture_tmp[0])
        msg.leftArmJointTemp.append( tempture_tmp[1])
        msg.leftArmJointTemp.append( tempture_tmp[2])
        msg.leftArmJointTemp.append( 0)
        msg.leftArmJointTemp.append( 0)

        msg.rightArmJointTemp.append( tempture_tmp[3])
        msg.rightArmJointTemp.append( tempture_tmp[4])
        msg.rightArmJointTemp.append( tempture_tmp[5])
        msg.rightArmJointTemp.append(0)
        msg.rightArmJointTemp.append(0)
    
        # 发布消息	
        servo_pub.publish(msg)
        # clear msg buffer

        del msg.leftArmJointAngle[:]
        del msg.rightArmJointAngle[:]
        del msg.leftArmJointPower[:]
        del msg.rightArmJointPower[:]
        del msg.leftArmJointTemp[:]
        del msg.rightArmJointTemp[:]
        # 清空临时变量
        angle_tmp = []
        power_tmp = []
        tempture_tmp = []
        
        pub_rate.sleep()
    # try:
    #     rospy.spin()   #  start ros publisher
    # except: KeyboardInterrupt:
    #      # ENDING THREADING of STATE PUB
    #     logging.info("THREADING - Thread: {} has done exit threadName.exit() !".format(threadName))


if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        rospy.loginfo('Robot Telemetry Info END od Publishing')
        rospy.logerr("dual_arm_manager exit()")



